using System;
using UnityEngine;
using System.Collections;

// Forms a circle using the centre of the herd as the midpoint
// and the captain as a point
public class CircleBehaviour : HerdSteeringBehaviour
{

  	// Distance from any other agent in the group an agent is allowed to be 
	// to still be considered in that group
	private const float agentMaxDistanceInGroup = 300.0f; 
	private const float minCirclingRadius = 250.0f; 
	private ArrayList agents;
	
	static private Vector3 herdCentre = new Vector3(0,0,0);
	static private float herdRadius = minCirclingRadius;
   
	public void Start ()
	{
		//Get a list of agents from the game controller so that it can remove and add agents at will
		this.agents = (((GameController)GameObject.
			FindWithTag("GameController").GetComponent("GameController")).getAgents());		
	}
	
	public void updateCaptainsOrders(HerderBehaviourScript herder)
	{
		if(agents == null)
			Start();
		
		ArrayList targetGroup = buildTargetGroup();
		
		Vector3 closest = new Vector3(0,0,0);
		float closestDist = 10000000.0f;
		
		Vector3 furthest = new Vector3(0,0,0);
		float furthestDist = 0.0f;
		
		// Get area from target group
		foreach(AgentBehaviourScript agent in targetGroup)
		{
			if (Vector3.Distance(herder.getPosition(), agent.getPosition()) > furthestDist)
			{
				furthestDist = Vector3.Distance(herder.getPosition(), agent.getPosition());
				furthest = agent.getPosition();
			}
			if (Vector3.Distance(herder.getPosition(), agent.getPosition()) < closestDist)
			{
				closestDist = Vector3.Distance(herder.getPosition(), agent.getPosition());
				closest = agent.getPosition();
			}
		}
		
		Vector3 midPoint = (closest+furthest)/2;
		float radius = Vector3.Distance(closest, furthest);
		//float radius = Vector3.Distance(getCaptainPosition(), furthest);
		radius *= 1.1f; // Give a bit of leeway to not freak the agents out 
		if (radius < minCirclingRadius)
			radius = minCirclingRadius;	
		
		const int pointsInCircle = 20;
		for (int i = 0; i < pointsInCircle; i++)
		{
			Vector3 thisPointOnCircle = new Vector3(0.0f, herder.transform.position.y, 0.0f);
			thisPointOnCircle.x = midPoint.x + radius * (float)Math.Sin( (Math.PI/180) * (i*(360/pointsInCircle)));
			thisPointOnCircle.z = midPoint.z + radius * (float)Math.Cos( (Math.PI/180) * (i*(360/pointsInCircle)));
			
			Vector3 nextPointOnCircle = new Vector3(0.0f, herder.transform.position.y, 0.0f);
			nextPointOnCircle.x = midPoint.x + radius * (float)Math.Sin( (Math.PI/180) * ((i+1)*(360/pointsInCircle)));
			nextPointOnCircle.z = midPoint.z + radius * (float)Math.Cos( (Math.PI/180) * ((i+1)*(360/pointsInCircle)));
			
			Debug.DrawLine(thisPointOnCircle, nextPointOnCircle, Color.red);
		}
		
		herdCentre = midPoint;
		herdRadius = radius;
	}

	// This function is very expensive but only has to run once per physics update,
	// We could probably get away with only doing it every second or so.
	private ArrayList buildTargetGroup()
	{
		ArrayList targetGroup = new ArrayList();

		targetGroup.Add(getNearestAgent(getCaptainPosition()));
		
		int positionInArray = 0;
		while(positionInArray < targetGroup.Count)
		{
			ArrayList toAdd = getAgentsNearby(agentMaxDistanceInGroup, ((AgentBehaviourScript)(targetGroup[positionInArray])).getPosition());
			foreach(AgentBehaviourScript agent in toAdd)
			{
				if (!targetGroup.Contains(agent))
			    {
			    	targetGroup.Add(agent);
				}
			}
			positionInArray++;
		}
		return targetGroup;
	}
	
	private ArrayList getAgentsNearby(float maxDistance, Vector3 position)
	{
		ArrayList nearbyAgents = new ArrayList();
        foreach (AgentBehaviourScript agent in agents) 
		{
			
			if (Vector3.Distance(position, agent.getPosition()) < maxDistance)
		    {
				nearbyAgents.Add(agent);	
			}
        }			
		return nearbyAgents;
	}
	
	private AgentBehaviourScript getNearestAgent(Vector3 position)
	{
		float agent_dist = 0.0f;
		AgentBehaviourScript nearestAgent = null;
		
        foreach (AgentBehaviourScript agent in agents) 
		{
			float temp_dist = Vector3.Distance(position, agent.getPosition());
			if(agent_dist == 0.0f || temp_dist < agent_dist)
			{
				agent_dist = temp_dist;
				nearestAgent = agent;
			}
        }		
		return nearestAgent;
	}
	
		
	public override Vector3 getSteering (HerderBehaviourScript herder)
	{
		
		float angleOfHerderOnCircle = herder.herder_num*(360/(herder.getNumHerders()-1 ));
		Vector3 thisPointOnCircle = new Vector3(0.0f, getHerdCentre().y, 0.0f);
		
		thisPointOnCircle.x = getHerdCentre().x + getHerdRadius() * (float)Math.Sin( (Math.PI/180) * angleOfHerderOnCircle);
		thisPointOnCircle.z = getHerdCentre().z + getHerdRadius() * (float)Math.Cos( (Math.PI/180) * angleOfHerderOnCircle);
		
		float distanceToCentre = Vector3.Distance(herder.getPosition(), getHerdCentre());
		float distanceToPoint = Vector3.Distance(herder.getPosition(), thisPointOnCircle);
		
		if (distanceToCentre < getHerdRadius()*0.9f)
		{
			return herder.getPosition() + ((herder.getPosition() - getHerdCentre()).normalized * getHerdRadius() *1.0f);
		}
			
		/* using a^2 + b^2 = c^2  to determine if we are on the side of the cirlce we would like to be
			if we are on the wrong side we need to navigate around */
		if ( distanceToPoint > Math.Sqrt( distanceToCentre*distanceToCentre + getHerdRadius()*getHerdRadius()))
		{
			// untiy uses positive angle for clockwise rotation from above
			if ( Vector3.Angle(herder.getPosition(), thisPointOnCircle) > 
			    	Vector3.Angle(herder.getPosition(), getHerdCentre()) )
		    {
		    	// Angle that is perpendicular, ie side of circle to nagivate around 
		    	angleOfHerderOnCircle = Vector3.Angle(herder.getPosition(), getHerdCentre()) + 90; 
			}else
			{
		    	// Angle that is perpendicular, ie side of circle to nagivate around 
		    	angleOfHerderOnCircle = Vector3.Angle(herder.getPosition(), getHerdCentre()) - 90; 
			}
			thisPointOnCircle.x = getHerdCentre().x + getHerdRadius()*1.2f * (float)Math.Sin( (Math.PI/180) * angleOfHerderOnCircle);
			thisPointOnCircle.z = getHerdCentre().z + getHerdRadius()*1.2f * (float)Math.Cos( (Math.PI/180) * angleOfHerderOnCircle);
		}
		
		
		return thisPointOnCircle;
	}
	
	public Vector3 getHerdCentre()
	{
		//all the work is done by the captain
		return herdCentre;	
	}
	
	public float getHerdRadius()
	{
		return herdRadius;	
	}
	
	Vector3 getCaptainPosition()
	{
		return (this.GetComponent("HerderBehaviourScript") as HerderBehaviourScript).getCaptain().getPosition();
	}


}


